package ru.ifmo.metopt;

import ru.ifmo.metopt.abstracts.*;

/**
 * @author avhaliullin
 */
public class PolakaRiberaSolver implements MultipleDimensionSolver {
    private static final double EPS = 1e-9;
    private final SingleDimensionSolver solver;
    private final GradientEncounter gradientEncounter;
    private final double q;

    public PolakaRiberaSolver(SingleDimensionSolver solver, GradientEncounter gradientEncounter, double q) {
        this.solver = solver;
        this.gradientEncounter = gradientEncounter;
        this.q = q;
    }

    private double[] sub(double[] a, double[] b) {
        double[] res = new double[b.length];
        if (a == null) {
            for (int i = 0; i < b.length; i++) {
                res[i] = -b[i];
            }
        } else {
            for (int i = 0; i < b.length; i++) {
                res[i] = a[i] - b[i];
            }
        }
        return res;
    }

    private double[] sum(double[] a, double[] b) {
        double[] res = new double[b.length];
        for (int i = 0; i < b.length; i++) {
            res[i] = a[i] + b[i];
        }
        return res;
    }

    private double weight(double[] a) {
        return Math.pow(scalProd(a, a), 0.5);
    }

    private double distance(double[] a, double[] b) {
        double res = 0;
        for (int i = 0; i < b.length; i++) {
            res += Math.abs(a[i] - b[i]);
        }
        return res / a.length;
    }

    private double scalProd(double[] a, double[] b) {
        double res = 0;
        for (int i = 0; i < b.length; i++) {
            res += a[i] * b[i];
        }
        return res;
    }

    private double[] mul(double[] vector, double scalar) {
        double[] res = vector.clone();
        for (int i = 0; i < res.length; i++) {
            res[i] *= scalar;
        }
        return res;
    }

    @Override
    public double[] solve(final Function f, double[] initial) {
        double[] prev = null;
        double[] current = initial;
        double[] antiGrad = new double[initial.length];
        double[] grad = antiGrad;
        double[] prevCoGrad = antiGrad;
        while (prev == null || distance(prev, current) > q) {
            double[] prevGrad = grad;
            grad = gradientEncounter.gradient(f, current);
            antiGrad = sub(null, grad);
            double prevGrad2 = scalProd(prevGrad, prevGrad);
            double betta = prevGrad2 < EPS ? 0 : Math.abs(scalProd(grad, sub(grad, prevGrad)) / scalProd(prevGrad, prevCoGrad));
            final double[] coGrad = sum(antiGrad, mul(prevCoGrad, betta));
            prevCoGrad = coGrad;
            final double[] finalPrev = current.clone();
            final double gradWeight = weight(coGrad);
            double localSolve = solver.solve(new Function1D() {
                @Override
                public double f(double arg) {
                    double[] point = new double[coGrad.length];
                    for (int i = 0; i < point.length; i++) {
                        point[i] = coGrad[i] * arg / gradWeight + finalPrev[i];
                    }
                    return f.f(point);
                }
            });
            prev = current.clone();
            for (int i = 0; i < current.length; i++) {
                current[i] += coGrad[i] * localSolve / gradWeight;
            }
        }
        return current;
    }
}
